import time
from machine import Pin, I2C

MPU_SELF_TESTX_REG = 0X0D
MPU_SELF_TESTY_REG = 0X0E
MPU_SELF_TESTZ_REG = 0X0F
MPU_SELF_TESTA_REG = 0X10
MPU_SAMPLE_RATE_REG = 0X19
MPU_CFG_REG = 0X1A
MPU_GYRO_CFG_REG = 0X1B
MPU_ACCEL_CFG_REG = 0X1C
MPU_MOTION_DET_REG = 0X1F
MPU_FIFO_EN_REG = 0X23
MPU_I2CMST_CTRL_REG = 0X24
MPU_I2CSLV0_ADDR_REG = 0X25
MPU_I2CSLV0_REG = 0X26
MPU_I2CSLV0_CTRL_REG = 0X27
MPU_I2CSLV1_ADDR_REG = 0X28
MPU_I2CSLV1_REG = 0X29
MPU_I2CSLV1_CTRL_REG = 0X2A
MPU_I2CSLV2_ADDR_REG = 0X2B
MPU_I2CSLV2_REG = 0X2C
MPU_I2CSLV2_CTRL_REG = 0X2D
MPU_I2CSLV3_ADDR_REG = 0X2E
MPU_I2CSLV3_REG = 0X2F
MPU_I2CSLV3_CTRL_REG = 0X30
MPU_I2CSLV4_ADDR_REG = 0X31
MPU_I2CSLV4_REG = 0X32
MPU_I2CSLV4_DO_REG = 0X33
MPU_I2CSLV4_CTRL_REG = 0X34
MPU_I2CSLV4_DI_REG = 0X35

MPU_I2CMST_STA_REG = 0X36
MPU_INTBP_CFG_REG = 0X37
MPU_INT_EN_REG = 0X38
MPU_INT_STA_REG = 0X3A

MPU_ACCEL_XOUTH_REG = 0X3B
MPU_ACCEL_XOUTL_REG = 0X3C
MPU_ACCEL_YOUTH_REG = 0X3D
MPU_ACCEL_YOUTL_REG = 0X3E
MPU_ACCEL_ZOUTH_REG = 0X3F
MPU_ACCEL_ZOUTL_REG = 0X40

MPU_TEMP_OUTH_REG = 0X41
MPU_TEMP_OUTL_REG = 0X42

MPU_GYRO_XOUTH_REG = 0X43
MPU_GYRO_XOUTL_REG = 0X44
MPU_GYRO_YOUTH_REG = 0X45
MPU_GYRO_YOUTL_REG = 0X46
MPU_GYRO_ZOUTH_REG = 0X47
MPU_GYRO_ZOUTL_REG = 0X48

MPU_I2CSLV0_DO_REG = 0X63
MPU_I2CSLV1_DO_REG = 0X64
MPU_I2CSLV2_DO_REG = 0X65
MPU_I2CSLV3_DO_REG = 0X66

MPU_I2CMST_DELAY_REG = 0X67
MPU_SIGPATH_RST_REG = 0X68
MPU_MDETECT_CTRL_REG = 0X69
MPU_USER_CTRL_REG = 0X6A
MPU_PWR_MGMT1_REG = 0X6B
MPU_PWR_MGMT2_REG = 0X6C
MPU_FIFO_CNTH_REG = 0X72
MPU_FIFO_CNTL_REG = 0X73
MPU_FIFO_RW_REG = 0X74
MPU_DEVICE_ID_REG = 0X75

MPU_ADDR = 0X68


class MPU6050:
    def __init__(self, sclIO, sdaIO):
        self.i2c = I2C(1, scl=Pin(sclIO), sda=Pin(sdaIO))
        # 初始化mpu6050
        self.WriteByte(MPU_PWR_MGMT1_REG, 0x80)
        time.sleep_ms(100)
        self.WriteByte(MPU_PWR_MGMT1_REG, 0x00)
        self.SetGyroFsr(3)  # 设置陀螺仪量程
        self.SetAccelFsr(0)  # 设置加速度量程
        self.SetRate(50)
        self.WriteByte(MPU_INT_EN_REG, 0x00)  # 关闭所有中断
        self.WriteByte(MPU_USER_CTRL_REG, 0x00)  # iic主模式关闭
        self.WriteByte(MPU_FIFO_EN_REG, 0x00)  # 关闭FIFO
        self.WriteByte(MPU_INTBP_CFG_REG, 0x80)  # INT引脚低电平有效

    # 设置陀螺仪量程
    def SetGyroFsr(self, fsr):
        self.WriteByte(MPU_GYRO_CFG_REG, fsr << 3)

    # 设置加速度量程
    def SetAccelFsr(self, fsr):
        self.WriteByte(MPU_ACCEL_CFG_REG, fsr << 3)

    def SetRate(self, rate):
        if rate > 1000:
            rate = 1000
        if rate < 4:
            rate = 4
        data = int(1000 / rate - 1)
        self.WriteByte(MPU_SAMPLE_RATE_REG, data)
        self.SetLPF(rate / 2)

    def SetLPF(self, lpf):
        data = 0
        if lpf >= 188:
            data = 1
        elif lpf >= 98:
            data = 2
        elif lpf >= 42:
            data = 3
        elif lpf >= 20:
            data = 4
        elif lpf >= 10:
            data = 5
        else:
            data = 6
        self.WriteByte(MPU_CFG_REG, data)

    # 获取温度
    def GetTemp(self):
        buf = self.ReadLen(MPU_TEMP_OUTH_REG, 2)
        raw = self.bytes_toint(buf[0], buf[1])
        temp = 36.53 + raw / 340
        return temp

    # 获取陀螺仪的值
    def GetGyro(self):
        buf = self.ReadLen(MPU_GYRO_XOUTH_REG, 6)
        gx = self.bytes_toint(buf[0], buf[1])
        gy = self.bytes_toint(buf[2], buf[3])
        gz = self.bytes_toint(buf[4], buf[5])
        return gx, gy, gz

    # 获取加速度的值
    def GetAccel(self):
        buf = self.ReadLen(MPU_ACCEL_XOUTH_REG, 6)
        ax = self.bytes_toint(buf[0], buf[1])
        ay = self.bytes_toint(buf[2], buf[3])
        az = self.bytes_toint(buf[4], buf[5])
        return ax, ay, az

    def WriteByte(self, reg, data):
        self.i2c.writeto_mem(MPU_ADDR, reg, bytearray([data]))

    def ReadByte(self, reg):
        buf = self.i2c.readfrom_mem(MPU_ADDR, reg, 1)
        return bytearray(buf)

    # 向reg写入多个字符buf
    def WriteLen(self, reg, buf):
        self.i2c.writeto_mem(MPU_ADDR, reg, bytearray([buf]))

    # 从reg读取len个字符
    def ReadLen(self, reg, len):
        buf = self.i2c.readfrom_mem(MPU_ADDR, reg, len)
        return bytearray(buf)

    # 将byte数据额转化为short类型
    def bytes_toint(self, firstbyte, secondbyte):
        if not firstbyte & 0x80:
            return firstbyte << 8 | secondbyte
        return - (((firstbyte ^ 255) << 8) | (secondbyte ^ 255) + 1)
